#include "ardetector.h"
#include <iostream>
using namespace std;
ArDetector::ArDetector():CBaseDetector("ArDetector", false){

    double fx,fy,cx,cy,k1,k2,k3,p1,p2;
    fx=955.8925;
    fy=955.4439;
    cx=296.9006;
    cy=215.9074;
    k1=-0.1523;
    k2=0.7722;
    k3=0;
    p1=0;
    p2=0;

  cameraMatrix = (cv::Mat_<float>(3, 3) <<
        fx, 0.0, cx,
        0.0, fy, cy,
        0.0, 0.0, 1.0);

  distCoeffs = (cv::Mat_<float>(5, 1) << k1, k2, p1, p2, k3);
  dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
}

int ArDetector::getResult(std::vector<pose> &poses){

    /**
     *  在这里将识别结果返回出去
     */
    poses.clear();
    if(retRvecs.size() == 0 && tVecs.size() == 0 )
        return -1;

    int count = retRvecs.size();
    for(int i = 0; i < count; i++)
    {
        pose p;
        p.objectName= std::to_string(i);
        p.position.x = tVecs[i][0];
        p.position.y = tVecs[i][1];
        p.position.z = tVecs[i][2];

        // need eular change to  quaternion
        poses.push_back(p);

//        p.quaternion

    }

    return 0;
}

int ArDetector::detection(){

    /**
     *  在这里进行你具体的检测算法，如果检测到成功，返回0，失败则返回-1
     */
   std::cout << "正在检测目标" << std::endl;


   //clear cache
   retRvecs.clear();
   tVecs.clear();

   //
   int ret = -1;
   std::vector<int> ids;
   std::vector<std::vector<cv::Point2f> > corners;
   cv::aruco::detectMarkers(_colorImg, dictionary, corners, ids);//检测靶标

   // if at least one marker detected
   if (ids.size() > 0) {
       cv::aruco::drawDetectedMarkers(preImage, corners, ids);//绘制检测到的靶标的框

       cv::aruco::estimatePoseSingleMarkers(corners, 0.055, cameraMatrix, distCoeffs, retRvecs, tVecs);//求解旋转矩阵rvecs和平移矩阵tvecs
       //cout<<"R :"<<rvecs[0]<<endl;
//       cout<<"T :"<<tvecs[0]<<endl;
       // draw axis for each marker
       for(int i=0; i<ids.size(); i++)
           cv::aruco::drawAxis(preImage, cameraMatrix, distCoeffs, retRvecs[i], tVecs[i], 0.1);


       ret = 0;
       std::cout << "检测失败" << std::endl;
   }else
   {
        std::cout << "检测完成" << std::endl;
   }
    return ret;
}

void ArDetector::setColorImg(const cv::Mat &inputImg){

    /**
     *  保存由框架传递下来的彩色图像
     */
    _colorImg = inputImg.clone();

    if(_colorImg.channels() == 3 )
    {
        cv::cvtColor(_colorImg, monoImage, cv::COLOR_BGR2GRAY);
    }else{
        monoImage = _colorImg;
    }

}

void ArDetector::setDepthImg(const cv::Mat &inputImg){

    /**
     *  保存由框架传递下来的深度图像
     */
    _depthrImg = inputImg;
}

int ArDetector::loadData(const std::string path, const std::string objectName){

    /**
     *  在这里根据框架传递下来的路径和物体名称来获取相关的配置
     *  如：网络权重、Linemod模板等
     */
    return 0;
}

int ArDetector::parseConfig(const YAML::Node &node){

    /**
     *  在这里解析由框架传递下来的配置信息
     */
    return 0;
}

int ArDetector::getPreImg(cv::Mat &preImg){

    preImg = this->preImage;
    /**
     *  在这里生成并返回预览图片
     */
    return 0;
}

H_EXPORT_PLUGIN(ArDetector, "ArDetector", "1.0")
